# -*- coding: utf-8 -*-

import numpy as np
from scipy.spatial import Delaunay, KDTree

def str2arr(obstacle_s):
    obstacle = []
    for s in obstacle_s.split():
        obstacle.append(float(s))
    obstacle = np.array(obstacle)
    obstacle = obstacle.reshape((int(obstacle.shape[0]/2), 2))
    obstacle = obstacle.tolist()
    return obstacle

WIDTH = 10
HEIGHT = 10

# 点的坐标
points = []

for i in range(WIDTH):
    for j in range(HEIGHT):
        points.append((i+0.5, j+0.5))

points = np.array(points + [
    
])

# 代表障碍区域的坐标
obstacle = str2arr("""
3.1653225806451615 1.9534632034632042
3.1249999999999996 1.2229437229437234
2.6814516129032255 0.7900432900432905
2.1370967741935485 0.925324675324676


6.25 7.148268398268399
5.866935483870968 6.850649350649352
6.088709677419354 6.228354978354979
5.907258064516128 5.876623376623378
7.17741935483871 5.76839826839827
6.834677419354838 6.282467532467534
7.23790322580645 6.904761904761905
6.774193548387096 7.256493506493507
8.004032258064516 7.148268398268399
8.104838709677418 6.715367965367966
7.782258064516128 6.363636363636365
8.14516129032258 5.876623376623378
8.084677419354838 5.146103896103897
7.762096774193548 4.767316017316018
7.096774193548386 5.146103896103897
6.91532258064516 4.821428571428573
6.834677419354838 4.361471861471863

1.1309523809523805 8.14935064935065
0.6980519480519467 7.770562770562771
0.7521645021645016 7.283549783549784
1.1309523809523805 6.688311688311689
1.9426406926406923 6.7694805194805205
1.9155844155844148 6.363636363636365
1.212121212121211 5.714285714285715
1.212121212121211 5.714285714285715
0.6709956709956701 6.120129870129871
2.2673160173160163 5.876623376623378
3.024891774891774 5.687229437229439
3.1060606060606055 5.227272727272728

""")

obstacle = np.array(obstacle + [
    [3., 2.5]
])

# 德劳三角剖分的结果
tri = Delaunay(points)
# 三角形的质心
center = np.zeros((tri.simplices.shape[0], 2))
for i in range(3):
    center += points[tri.simplices[:, i]]
center /= 3
# 质心的KD树版
kdcenter = KDTree(center)

# 障碍物三角形的下标
obtri = set()

for pos in obstacle:
    obtri.add(kdcenter.query(pos)[1])

# 可通行三角的下标
freetri = set(range(tri.simplices.shape[0]))
freetri = freetri - obtri

obtri = np.array(list(obtri), dtype=np.int32)
freetri = np.array(list(freetri), dtype=np.int32)